using System;
using System.Collections.Generic;
using System.Text;

namespace ElteIk.RoboSoccer.Core.Physics
{
	/// <summary>
	/// Standard object that is driven by two motors.
	/// </summary>
    public class TwoWheelDrivenObject : Polygon, IRobotController
    {
		#region Lifecycle

		public TwoWheelDrivenObject(Vector2D center)
			: base(center)
		{
			motorDistance = 1.5f;
			motorStrength = 1f;
            maxPower = 10.0f;
            isBackward = false;
		}

		#endregion

		#region Motor Properties
		
		private float motorDistance;

		/// <summary>
		/// The distance of the motor from the center of the object.
		/// </summary>
		public float MotorDistance
		{
			get { return motorDistance; }
			set { motorDistance = value; }
		}

		private float motorStrength;
		/// <summary>
		/// The multiplier of the motor settings.
		/// </summary>
		public float MotorStrength
		{
			get { return motorStrength; }
			set { motorStrength = value; }
		}

        private float maxPower;

        /// <summary>
        /// The maximum power of the motors.
        /// </summary>
        public float MaxPower
        {
            get { return maxPower; }
            set { maxPower = value; }
        }

		#endregion

		#region Force calculation

        private void LimitMotors()
        {
            if (this.motorLeft < 0f)
            {
                this.motorLeft = Math.Max(this.motorLeft, -this.maxPower);
            }
            else if (this.motorLeft > 0f)
            {
                this.motorLeft = Math.Min(this.motorLeft, this.maxPower);
            }

            if (this.motorRight < 0f)
            {
                this.motorRight = Math.Max(this.motorRight, -this.maxPower);
            }
            else if (this.motorRight > 0f)
            {
                this.motorRight = Math.Min(this.motorRight, this.maxPower);
            }
        }

        /// <summary>
        /// Calculates the forces that the motors are producing.
        /// </summary>
        protected virtual IList<FreeVector2D> ApplyMotorForces()
        {
            List<FreeVector2D> forces = new List<FreeVector2D>();
            LimitMotors();

            if (this.motorLeft != 0)
            {
                forces.Add(new FreeVector2D(
                    this.Heading.Normalized.Perpendicular * this.motorDistance,
                    this.Heading.Normalized * this.motorLeft * this.motorStrength));
            }
            if (this.motorRight != 0)
            {
                forces.Add(new FreeVector2D(
                    this.Heading.Normalized.Perpendicular * -this.motorDistance,
                    this.Heading.Normalized * this.motorRight * this.motorStrength));
            }

            return forces;
        }

        public override IList<FreeVector2D> Forces
        {
            get
            {
                List<FreeVector2D> forces = new List<FreeVector2D>(base.Forces);

                forces.AddRange(ApplyMotorForces());

                // It is the physical engine's responsibility to erase the motor's states after calculation is performed.
                return forces;
            }
        }
		
		#endregion

		#region IRobotController Members

		private float motorLeft;

		/// <summary>
		/// The current power of the left motor.
		/// </summary>
		public float MotorLeft
		{
			get
			{
				return motorLeft;
			}
			set
			{
				motorLeft = value;
			}
		}

		private float motorRight;

		/// <summary>
		/// The current power of the right motor.
		/// </summary>
		public float MotorRight
		{
			get
			{
				return motorRight;
			}
			set
			{
				motorRight = value;
			}
		}

        private float brake;

        /// <summary>
        /// The state of the brakes.
        /// </summary>
        public float Brake 
        {
            get { return brake; }
            set { brake = value; }
        }


		#endregion

        public void Update(float dt)
        {
        }

        private bool isBackward;

        /// <summary>
        /// The vehicle goes backward.
        /// </summary>
        public bool IsBackward
        {
            get { return isBackward; }
            set { isBackward = value; }
        }
    }
}
